#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "rplidar_ros/Angle.h"
#include <serial/serial.h>
#include <math.h>

#define RAD2DEG(x) ((x)*180./M_PI)
#define DEG2RAD(x) ((x)*M_PI/180.)
#define UN std::numeric_limits<float>::infinity()
#define PI 3.1415926
ros::Publisher pub;
serial::Serial sp;
int allcount = 0;
struct point
{
    float angle;
    float range;
};   

point p[1500];
point point_rem;

void intTocahr(char* c1, char* c2, char* c3, char* c4, int x)
{
	if(x>0)
	{
		*c1 = '0' + x/1000%10;
		*c2 = '0' + x/100%10;
		*c3 = '0' + x/10%10;
		*c4 = '0' + x%10;
	}
	else
	{
		*c1 = '0' - x/1000%10;
		*c2 = '0' - x/100%10;
		*c3 = '0' - x/10%10;
		*c4 = '0' - x%10;
			
	}
}
/**********/
/*高斯滤波*/
/**********/
float s[5]={0};
int i=0;
float gt[5], gt_sum = 0;			//高斯权重
float sigma = 5;					//标准差
float gaussfliter(float slope)
{
	float sr = 0;
	s[4] = slope;
	for(i=0; i<5; i++)
	{
		sr += s[i]*gt[i];
		if(i!=4)s[i]=s[i+1];
	}
	return sr;
}

/***********************/
/*限制角度在-45到45之间*/
/***********************/
float limit(float angle)
{
    while(angle>45)angle = angle - 90;
    while(angle<-45)angle =angle + 90;
    return angle;
}
/**********/
/*计算斜率*/
/**********/
float calculate(point p1, point p2)		
{
    float a = p2.range*cos(DEG2RAD(p2.angle))-p1.range*cos(DEG2RAD(p1.angle));
    float b = p1.range*sin(DEG2RAD(p1.angle))-p2.range*sin(DEG2RAD(p2.angle));
    float result = limit(RAD2DEG(atan(a/b)));
    return result;
}
/**********/
/*计算长度*/
/**********/
float length(point p1, point p2)		
{
    float ang = abs(p1.angle-p2.angle);
    float len = sqrt(p1.range*p1.range+p2.range*p2.range-2*p1.range*p2.range*cos(DEG2RAD(ang)));
    return len;
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int count = scan->scan_time / scan->time_increment;
    int count_now = 0, count_rem = 0, i_first = 0, i_last = 0, first = 0, last = 0;
    float slope_now = 0, slope_rem = 0, slope = 0;
    float slope_sum = 0;
    float angle_rem = 0;
    float length_rem = 0, len = 0;
	float distance1 = 0, distance2 = 0, distance3 = 0;
    
    for(int i = 0; i < count; i++) {
        p[i].angle = RAD2DEG(scan->angle_increment * i);
        p[i].range = scan->ranges[i];
		if(p[i].angle <0.5 && p[i].angle>0)
		{
			distance1 = p[i].range;
			if(distance1 != UN)
			{
				char m[8]={'~', 'L', 0, 0, 0, 0, '\r', '\n'}; 
				intTocahr(m+2, m+3, m+4, m+5, (int)(distance1*1000));
				sp.write((const uint8_t*)m, sizeof(m));
				ROS_INFO("ANGLE:[%f], DISTANCE1:[%f]", p[i].angle, distance1);
			}
		}
		if(p[i].angle <180.5 && p[i].angle>179.5)
		{
			distance2 = p[i].range;
			if(distance2 != UN)
			{
				char m[8]={'~', 'R', 0, 0, 0, 0, '\r', '\n'}; 
				intTocahr(m+2, m+3, m+4, m+5, (int)(distance2*1000));
				sp.write((const uint8_t*)m, sizeof(m));
				ROS_INFO("ANGLE:[%f], DISTANCE2:[%f]", p[i].angle, distance2);
			}
		}
		if(p[i].angle <90.5 && p[i].angle>89.5)
		{
			distance3 = p[i].range;
			if(distance3 != UN)
			{
				char m[8]={'~', 'B', 0, 0, 0, 0, '\r', '\n'}; 
				intTocahr(m+2, m+3, m+4, m+5, (int)(distance3*1000));
				sp.write((const uint8_t*)m, sizeof(m));
				ROS_INFO("ANGLE:[%f], DISTANCE3:[%f]", p[i].angle, distance3);
			}
		}
    }
    for(int i = 0; i < count; i++) {
        if(p[i].range != UN &&
           i>1 &&
           i<count-2 &&
           p[i-1].range != UN  && 
           p[i+1].range != UN ){	//判断连续点
               float k1 = p[i-1].range*p[i+1].range*sin(DEG2RAD(p[i+1].angle-p[i-1].angle));
               float k2 = p[i].range*(p[i-1].range*sin(DEG2RAD(p[i].angle-p[i-1].angle))-p[i+1].range*sin(DEG2RAD(p[i].angle-p[i+1].angle)));
               float dk = abs(k1 - k2);
               if(dk<0.0005 && p[i].range>0.6 && p[i].angle > 25 && p[i].angle < 135){
                   count_now++;
                   if(count_now == 1){			//记录起点
                       i_first = i;
                   }
               }
               else{
                   //if(count_now > count_rem){		//更新最长直线
				   if(count_now){
					len=length(p[i-1], p[i_first]);
					if(len>length_rem){
					   i_last = i-2;
                       count_rem = count_now;
                       length_rem = len;
                       if((i_last-i_first+1)%2)		//项数为奇数
                       {
						for(int j = i_first; j<(i_last+i_first)/2; j++)
                       	{
                       		slope_sum += calculate(p[j], p[(i_last-i_first)/2+j+1]);
						}
						slope_rem = slope_sum / ((i_last-i_first)/2);
                       }
                       else					//项数为偶数
                       {
                       	for(int j = i_first; j<(i_last+i_first+1)/2; j++)
                       	{
                       		slope_sum += calculate(p[j], p[(i_last-i_first+1)/2+j]);
						}
						slope_rem = slope_sum / ((i_last-i_first+1)/2);
                       }
					   first = i_first;
					   last = i_last;
					   //ROS_INFO("ANGLE:[%f] first:[%f] last:[%f]", slope_rem, p[first].angle, p[last].angle);
					}
                   }
                   count_now = 0;
                   slope_sum = 0;
				   len = 0;
               }
           }
           else{				
               //if(count_now > count_rem){			//更新最长直线
			   if(count_now){
				len=length(p[i-1], p[i_first]);
				if(len>length_rem){
                   i_last = i-2;
                   count_rem = count_now;
                   length_rem = len;
				   if((i_last-i_first+1)%2)		//项数为奇数
				   {
					for(int j = i_first; j<(i_last+i_first)/2; j++)
					{
						slope_sum += calculate(p[j], p[(i_last-i_first)/2+j+1]);
					}
					slope_rem = slope_sum / ((i_last-i_first)/2);
				   }
				   else					//项数为偶数
				   {
					for(int j = i_first; j<(i_last+i_first+1)/2; j++)
					{
						slope_sum += calculate(p[j], p[(i_last-i_first+1)/2+j]);
					}
					slope_rem = slope_sum / ((i_last-i_first+1)/2);
				   }
				   first = i_first;
				   last = i_last;
				   //ROS_INFO("ANGLE:[%f] first:[%f] last:[%f]", slope_rem, p[first].angle, p[last].angle);
				}
               }
               count_now = 0;
               slope_sum = 0;
			   len = 0;
           }
    }
    if(allcount != 4)
    {
        allcount++;
        s[allcount] = slope_rem;
    }
    else
    {
	slope = gaussfliter(slope_rem);
    }
	//slope = slope_rem;
	//ROS_INFO("ANGLE:[%f] first:[%f] last:[%f]", slope_rem, p[first].angle, p[last].angle);
    ROS_INFO("ANGLE2:[%f]", slope);
    rplidar_ros::Angle angle_msg;
    angle_msg.angle1 = slope_rem;
    angle_msg.angle2 = slope;
    pub.publish(angle_msg);
    if(slope>0)
    {
    	char m[8]={'~', 'A', 0, 0, 0, 0, '\r', '\n'}; 
    	intTocahr(m+2, m+3, m+4, m+5, (int)(slope*100));
        sp.write((const uint8_t*)m, sizeof(m));
    }
    else
    {
    	char m[9]={'~', 'A', '-',  0, 0, 0, 0, '\r', '\n'}; 
    	intTocahr(m+3, m+4, m+5, m+6, (int)(slope*100));
        sp.write((const uint8_t*)m, sizeof(m));
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rplidar_node_client");
    ros::NodeHandle n;
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    sp.setPort("/dev/ch340");
    //设置串口通信的波特率
    sp.setBaudrate(115200);
    //串口设置timeout
    sp.setTimeout(to);
 
    try
    {
        //打开串口
        sp.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    
    //判断串口是否打开成功
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("/dev/ch340 is opened.");
    }
    else
    {
        return -1;
    }
    pub = n.advertise<rplidar_ros::Angle>("/angle", 10);
    for(i=0; i<5; i++)
	{
		gt[i] = exp(-(i-5)*(i-5)/(2*sigma*sigma))/(sigma*sqrt(2*PI));
		gt_sum += gt[i];
	}
	for(i=0; i<5; i++)
	{
		gt[i] = gt[i]/gt_sum;			//归一化
	}
    ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
    ros::spin();

    return 0;
}